
import isaaclab.sim as sim_utils
from isaaclab.actuators import DCMotorCfg, ImplicitActuatorCfg, DelayedPDActuatorCfg
from isaaclab.assets.articulation import ArticulationCfg

from legged_lab_v2.assets import ISAACLAB_ASSETS_DATA_DIR


LW_LEG_CFG = ArticulationCfg(
    spawn=sim_utils.UrdfFileCfg(
        fix_base=False,
        merge_fixed_joints=True,
        replace_cylinders_with_capsules=False,
        asset_path=f"{ISAACLAB_ASSETS_DATA_DIR}/Robots/LW/LW_leg_description/urdf/LW_leg.urdf",
        activate_contact_sensors=True,
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
            rigid_body_enabled=True,
            disable_gravity=False,
            retain_accelerations=False,
            linear_damping=0.0,
            angular_damping=0.0,
            max_linear_velocity=1000.0,
            max_angular_velocity=1000.0,
            max_depenetration_velocity=1.0,
        ),
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
            enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0
        ),
        joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg(
            gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=0, damping=0)
        ),
    ),
    init_state=ArticulationCfg.InitialStateCfg(
        pos=(0.0, 0.0, 0.8),
        joint_pos={
            "right_hip_joint": 0.0,
            "right_thigh_joint": -0.17453,
            "right_shank_joint": 0.34907,
            "right_wheel_joint": 0.0,
            "right_foot_joint": -0.17453,
            "left_hip_joint": 0.0,
            "left_thigh_joint": 0.17453,
            "left_shank_joint": -0.34907,
            "left_wheel_joint": 0.0,
            "left_foot_joint": 0.17453,
            # "right_hip_joint": 0.0,
            # "right_thigh_joint": 0.0,
            # "right_shank_joint": 0.0,
            # "right_wheel_joint": 0.0,
            # "right_foot_joint": 0.0,
            # "left_hip_joint": 0.0,
            # "left_thigh_joint": 0.0,
            # "left_shank_joint": 0.0,
            # "left_wheel_joint": 0.0,
            # "left_foot_joint": 0.0,
        },
        joint_vel={".*": 0.0},
    ),
    soft_joint_pos_limit_factor=0.9,
    actuators={
        "legs": DCMotorCfg(
            joint_names_expr=[
                "right_hip_joint",
                "right_thigh_joint",
                "right_shank_joint",
                "left_hip_joint",
                "left_thigh_joint",
                "left_shank_joint",
                ],
            effort_limit=120.0,
            saturation_effort=120.0,
            velocity_limit=20.0,
            stiffness=60.0,
            damping=2.25,
            friction=0.0,
        ),
        "wheel": DCMotorCfg(
            joint_names_expr=[
                "right_wheel_joint",
                "left_wheel_joint",
                ],
            effort_limit=40.0,
            saturation_effort=40.0,
            velocity_limit=33.0,
            stiffness=0.0,
            damping=0.8,
            friction=0.0,
        ),
        "foot": DCMotorCfg(
            joint_names_expr=[
                "right_foot_joint",
                "left_foot_joint",
                ],
            effort_limit=27.0,
            saturation_effort=27.0,
            velocity_limit=10.0,
            stiffness=15.0,
            damping=0.3,
            friction=0.0,
        ),
    },
)